#pragma once
#include <vector>
#include "gtest/gtest.h"
#include "DataStructure/Common/Constant.h"

#include "DBManager/DBServiceFactory.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Coordinate/ICORD1CService.h"
#include "DBManager/IService/Input/Coordinate/ICORD1RService.h"
#include "DBManager/IService/Input/Coordinate/ICORD1SService.h"
#include "DBManager/IService/Input/Coordinate/ICORD2CService.h"
#include "DBManager/IService/Input/Coordinate/ICORD2RService.h"
#include "DBManager/IService/Input/Coordinate/ICORD2SService.h"
#include "SGFem/Pre/Assembly/Assembly.h"

#include "../CoordinateProcessor.h"
using SG::DataStructure::Common::TOLERANCE;
using namespace SG::Algebra;
using SG::DataStructure::FEM::FORCEData;
using SG::DataStructure::FEM::Cartesian2Coord;
using namespace SG::DataStructure::FEM;

using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;

extern std::shared_ptr<DBServiceFactory> DB_SERVICEFACTORY;

TEST (CoordinateProcessorTest, Coordinate_Independent_Test)
{
    // Input
    DB_SERVICEFACTORY->ClearDatabase ();

    SG::DataStructure::FEM::NodeData node;
    node.m_id       = 1;
    node.m_defCoord = 0;
    node.m_x[0]     = 1.0;
    node.m_x[1]     = 2.0;
    node.m_x[2]     = 3.0;
    // DB_SERVICEFACTORY->get<INodeService>()->Add(node);
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id   = 2;
    node.m_x[0] = 4.0;
    node.m_x[1] = 5.0;
    node.m_x[2] = 6.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id   = 3;
    node.m_x[0] = 9.0;
    node.m_x[1] = 8.0;
    node.m_x[2] = 7.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id   = 4;
    node.m_x[0] = 0.0;
    node.m_x[1] = 0.0;
    node.m_x[2] = 0.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);

    auto  G11 = 1;
    auto  G21 = 2;
    auto  G31 = 3;
    Point GA{ 1.0, 2.0, 3.0 };
    Point GB{ 4.0, 5.0, 6.0 };
    Point GC{ 9.0, 8.0, 7.0 };

    CORD1RData cord1r;
    cord1r.m_id = 1;
    cord1r.m_g1 = G11;
    cord1r.m_g2 = G21;
    cord1r.m_g3 = G31;
    CORD1CData cord1c;
    cord1c.m_id = 2;
    cord1c.m_g1 = G11;
    cord1c.m_g2 = G21;
    cord1c.m_g3 = G31;
    CORD1SData cord1s;
    cord1s.m_id = 3;
    cord1s.m_g1 = G11;
    cord1s.m_g2 = G21;
    cord1s.m_g3 = G31;
    
    CORD2RData cord2r;
    cord2r.m_id = 4;
    cord2r.m_rId = 0;
    cord2r.m_a1  = GA.m_x;
    cord2r.m_a2  = GA.m_y;
    cord2r.m_a3  = GA.m_z;
    cord2r.m_b1  = GB.m_x;
    cord2r.m_b2  = GB.m_y;
    cord2r.m_b3  = GB.m_z;
    cord2r.m_c1  = GC.m_x;
    cord2r.m_c2  = GC.m_y;
    cord2r.m_c3  = GC.m_z;
    CORD2CData cord2c;
    cord2c.m_id = 5;
    cord2c.m_rId = 0;
    cord2c.m_a1  = GA.m_x;
    cord2c.m_a2  = GA.m_y;
    cord2c.m_a3  = GA.m_z;
    cord2c.m_b1  = GB.m_x;
    cord2c.m_b2  = GB.m_y;
    cord2c.m_b3  = GB.m_z;
    cord2c.m_c1  = GC.m_x;
    cord2c.m_c2  = GC.m_y;
    cord2c.m_c3  = GC.m_z;
    CORD2SData cord2s;
    cord2s.m_id = 6;
    cord2s.m_rId = 0;
    cord2s.m_a1  = GA.m_x;
    cord2s.m_a2  = GA.m_y;
    cord2s.m_a3  = GA.m_z;
    cord2s.m_b1  = GB.m_x;
    cord2s.m_b2  = GB.m_y;
    cord2s.m_b3  = GB.m_z;
    cord2s.m_c1  = GC.m_x;
    cord2s.m_c2  = GC.m_y;
    cord2s.m_c3  = GC.m_z;

    DB_SERVICEFACTORY->get<ICORD1RService> ()->Add (cord1r);
    DB_SERVICEFACTORY->get<ICORD1CService> ()->Add (cord1c);
    DB_SERVICEFACTORY->get<ICORD1SService> ()->Add (cord1s);
    DB_SERVICEFACTORY->get<ICORD2RService> ()->Add (cord2r);
    DB_SERVICEFACTORY->get<ICORD2CService> ()->Add (cord2c);
    DB_SERVICEFACTORY->get<ICORD2SService> ()->Add (cord2s);

    // 写入工况
    SG::DataStructure::FEM::Subcase subcase;
    subcase.AddParameter({"SUBCASE","1"});
    subcase.AddParameter({"SOL","101"});
    DB_SERVICEFACTORY->get<ISubcaseService> ()->Initial (subcase.Dump());

    // 初始化。 给节点编号指定索引
    auto& assembly = AssemblyEigen::GetInstance ();
    assembly.Clear ();
    assembly.Initialize (DB_SERVICEFACTORY, subcase, { 1 });

    auto& coordinateprocessor = CoordinateProcessor::GetInstance ();
    coordinateprocessor.Clear ();
    coordinateprocessor.Initialize (DB_SERVICEFACTORY);

    Point    OriginTarget{ 1.0, 2.0, 3.0 };
    Vector3D baseXTarget{ 0.707107, 0.0, -0.707107 };
    Vector3D baseYTarget{ -0.408248, 0.816497, -0.408248 };
    Vector3D baseZTarget{ 0.577350, 0.577350, 0.577350 };

    // Check
    for (std::size_t cId = 1; cId <= 6; ++cId)
    {
        auto coord = coordinateprocessor.GetCoord (cId);
        ASSERT_TRUE (NULL != coord);
        ASSERT_TRUE (coord->GetOrigin () == OriginTarget);
        ASSERT_TRUE (coord->GetBaseX () == baseXTarget);
        ASSERT_TRUE (coord->GetBaseY () == baseYTarget);
        ASSERT_TRUE (coord->GetBaseZ () == baseZTarget);
    }
}

namespace SG::FEMSolver{
// 三个坐标系相互依赖。 但是由于坐标系顺序问题。只会进行一轮遍历
TEST (CoordinateProcessorTest, Coordinate_dependent_Test)
{
    // Input
    DB_SERVICEFACTORY->ClearDatabase ();

    SG::DataStructure::FEM::NodeData node;
    node.m_id       = 1;
    node.m_defCoord = 0;
    node.m_x[0]     = 6.0;
    node.m_x[1]     = 8.0;
    node.m_x[2]     = 10.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id       = 2;
    node.m_defCoord = 0;
    node.m_x[0]     = 5.0;
    node.m_x[1]     = 2.0;
    node.m_x[2]     = 1.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id       = 3;
    node.m_defCoord = 0;
    node.m_x[0]     = 4.0;
    node.m_x[1]     = 9.0;
    node.m_x[2]     = 12.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id       = 4;
    node.m_defCoord = 100;
    node.m_x[0]     = 10.0;
    node.m_x[1]     = 20.0;
    node.m_x[2]     = 30.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id       = 6;
    node.m_defCoord = 100;
    node.m_x[0]     = 9.0;
    node.m_x[1]     = 6.0;
    node.m_x[2]     = 3.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id       = 8;
    node.m_defCoord = 200;
    node.m_x[0]     = 15.0;
    node.m_x[1]     = 12.0;
    node.m_x[2]     = 24.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);
    node.m_id       = 9;
    node.m_defCoord = 200;
    node.m_x[0]     = 14.0;
    node.m_x[1]     = 10.0;
    node.m_x[2]     = 8.0;
    DB_SERVICEFACTORY->get<INodeService> ()->Add (node);

    CORD1RData cord1;
    cord1.m_id = 100;
    cord1.m_g1 = 1;
    cord1.m_g2 = 2;
    cord1.m_g3 = 3;

    CORD1RData mixCord1;  // 此坐标系通过两个节点依赖 100号坐标系
    mixCord1.m_id = 200;
    mixCord1.m_g1 = 2;
    mixCord1.m_g2 = 6;
    mixCord1.m_g3 = 4;

    CORD1RData mixcord3;
    mixcord3.m_id = 300;
    mixcord3.m_g1 = 9;
    mixcord3.m_g2 = 8;
    mixcord3.m_g3 = 4;

    DB_SERVICEFACTORY->get<ICORD1RService> ()->Add (cord1);
    DB_SERVICEFACTORY->get<ICORD1RService> ()->Add (mixCord1);
    DB_SERVICEFACTORY->get<ICORD1RService> ()->Add (mixcord3);

    // 写入工况
    SG::DataStructure::FEM::Subcase subcase;
    subcase.AddParameter({"SUBCASE","1"});
    subcase.AddParameter({"SOL","101"});
    DB_SERVICEFACTORY->get<ISubcaseService> ()->Initial (subcase.Dump());

    auto& assembly = AssemblyEigen::GetInstance ();
    assembly.Clear ();
    // 内部不会调用坐标系计算，因为坐标系还没有传入 (目的是完成节点索引计算)
    assembly.Initialize (DB_SERVICEFACTORY, subcase, { 1 });

    auto& coordinateprocessor = CoordinateProcessor::GetInstance ();
    coordinateprocessor.Clear ();
    // 内部不会调用坐标系计算，因为坐标系还没有传入
    coordinateprocessor.Initialize (DB_SERVICEFACTORY);

    SG::DataStructure::FEM::ICoordinatePArray coordSet;

    // coordSet[100] = std::make_shared<CartesianCoord>(cord1.GetCartesianCoord());
    // coordSet[200] = std::make_shared<CartesianCoord>(mixCord1.GetCartesianCoord());
    // coordSet[200]->SetRefCoordId (1, 100);
    // coordSet[200]->SetRefCoordId (2, 100);
    // coordSet[300] = std::make_shared<CartesianCoord>(mixcord3.GetCartesianCoord());
    // coordSet[300]->SetRefCoordId (0, 200);
    // coordSet[300]->SetRefCoordId (1, 200);
    // coordSet[300]->SetRefCoordId (2, 100);

    // Test Data
    CoordinateProcessor::Compute (DB_SERVICEFACTORY, coordSet);

    // Check [100] coord
    Point    originTarget1{ 6.0, 8.0, 10.0 };
    Vector3D baseXTarget1{ -0.987904, -0.053607, 0.145505 };
    Vector3D baseYTarget1{ -0.124783, 0.831890, -0.540729 };
    Vector3D baseZTarget1{ -0.092057, -0.552344, -0.828517 };

    ASSERT_TRUE (coordSet[100]->GetOrigin () == originTarget1);
    ASSERT_TRUE (coordSet[100]->GetBaseX () == baseXTarget1);
    ASSERT_TRUE (coordSet[100]->GetBaseY () == baseYTarget1);
    ASSERT_TRUE (coordSet[100]->GetBaseZ () == baseZTarget1);

    // Check [200] coord
    Point    originTarget2{ 5.0, 2.0, 1.0 };
    Vector3D baseXTarget2{ -0.383882, 0.088842, -0.919098 };
    Vector3D baseYTarget2{ -0.638817, -0.744269, 0.194874 };
    Vector3D baseZTarget2{ -0.666743, 0.661944, 0.342466 };

    ASSERT_TRUE (coordSet[200]->GetOrigin () == originTarget2);
    ASSERT_TRUE (coordSet[200]->GetBaseX () == baseXTarget2);
    ASSERT_TRUE (coordSet[200]->GetBaseY () == baseYTarget2);
    ASSERT_TRUE (coordSet[200]->GetBaseZ () == baseZTarget2);

    // Check [300] coord
    Point    originTarget3{ -12.096478, 1.096660, -7.178900 };
    Vector3D baseXTarget3{ 0.002562, 0.476825, -0.878994 };
    Vector3D baseYTarget3{ -0.646191, -0.670038, -0.365357 };
    Vector3D baseZTarget3{ -0.763171, 0.568934, 0.306404 };

    ASSERT_TRUE (coordSet[300]->GetOrigin () == originTarget3);
    ASSERT_TRUE (coordSet[300]->GetBaseX () == baseXTarget3);
    ASSERT_TRUE (coordSet[300]->GetBaseY () == baseYTarget3);
    ASSERT_TRUE (coordSet[300]->GetBaseZ () == baseZTarget3);
}
};

class CoordinateProcessorTwoTrussCaseTest : public ::testing::Test
{
public:
    static void SetUpTestSuite ()
    {
        // Import
        Link_TwoEle model;
        DB_SERVICEFACTORY = model.GetModel ();

        // Initialize
        auto& coordinateprocessor = CoordinateProcessor::GetInstance();
        coordinateprocessor.Clear();
        coordinateprocessor.Initialize (DB_SERVICEFACTORY);
    }
};


TEST (CoordinateProcessorTest, GetCoordTrans)
{
    // Input data
    DB_SERVICEFACTORY->ClearDatabase();
    auto& coordinateprocessor = CoordinateProcessor::GetInstance ();
    coordinateprocessor.Clear ();
    Point PA{1.0, 2.0, 3.0};
    Point PB{4.0, 5.0, 6.0};
    Point PC{9.0, 8.0, 7.0};
    CORD2RData coordSys(1, 0, PA, PB, PC);
    // coordSys.CORD2RData(1, 0, PA, PB, PC);
    // coordSys.GetCartesian2Coord();
    //  (1, 0, PA, PB, PC);
    DB_SERVICEFACTORY -> get<ICORD2RService> () ->Add (coordSys);
    // Test data

    coordinateprocessor.Initialize (DB_SERVICEFACTORY);
    Matrix TTest (3, 3);
    coordinateprocessor.GetCoordTrans (1, Point(), _OUT TTest);

    // Target data
    Matrix TTarget (3, 3, { 0.707107, 0.0,      -0.707107,
                           -0.408248, 0.816497, -0.408248,
                            0.577350, 0.577350,  0.577350});

    // Check
    ASSERT_TRUE (TTarget == TTest);
}

// TEST (OperaterTest, ComputeCoord2BasicTest)
// {
//     // Input data
//     auto dbServiceFactory = std::make_shared<MemoryDBManager> ();

//     // 坐标系1
//     Point PA{1.0, 2.0, 3.0};
//     Point PB{2.0, 2.0, 3.0};
//     Point PC{1.0, 3.0, 3.0};
//     Cartesian2Coord coordSys (1, 0, PA, PB, PC);
//     dbServiceFactory -> AppendCoordinate (&coordSys);

//     auto& assembly = AssemblyEigen::GetInstance ();
//     assembly.Clear ();
//     assembly.Initialize (dbServiceFactory, 1, {1});
    
//     Point pointTest, pointTarget;
//     NodeData node;

//     // Basic to basic
//     {
//         // Input data
//         node = {1, 0, 0, -2.0, -3.0, -1.0};

//         // Test data
//         SG::FEMSolver::Element::ComputeCoord2Basic (node, _OUT pointTest);

//         // Target data
//         pointTarget = {-2.0, -3.0, -1.0};

//         // Check
//         ASSERT_TRUE (pointTarget == pointTest);
//     }

//     // Local to basic
//     {
//         // Input data
//         node = {1, 1, 0, -2.0, -3.0, -1.0};

//         // Test data
//         SG::FEMSolver::Element::ComputeCoord2Basic (node, _OUT pointTest);

//         // Target data
//         pointTarget = {0.0, 0.0, 0.0};

//         // Check
//         ASSERT_TRUE (pointTarget == pointTest);
//     }
// }